15. Radar Measurements
Radar Measurements
H versus h(x)
The H matrix from the lidar lesson and h(x) equations from the radar lesson are actually accomplishing the same thing; they are both needed to solve y = z - Hx' in the update step.
But for radar, there is no H matrix that will map the state vector x into polar coordinates; instead, you need to calculate the mapping manually to convert from cartesian coordinates to polar coordinates.
Here is the h function that specifies how the predicted position and speed get mapped to the polar coordinates of range, bearing and range rate.
Hence for radar y = z - Hx' becomes y = z - h(x') .
Definition of Radar Variables
- The range, ( \rho ), is the distance to the pedestrian. The range is basically the magnitude of the position vector \rho which can be defined as \rho = sqrt(p_x^2 + p_y^2) .
- \varphi = atan(p_y / p_x) . Note that \varphi is referenced counter-clockwise from the x-axis, so \varphi from the video clip above in that situation would actually be negative.
- The range rate, \dot{\rho} , is the projection of the velocity, v , onto the line, L .
Deriving the Radar Measurement Function
The measurement function is composed of three components that show how the predicted state, x' = (p_x', p_y', v_x', v_y')^T , is mapped into the measurement space, z = (\rho, \varphi, \dot{\rho})^T :
The range, \rho , is the distance to the pedestrian which can be defined as:
\varphi is the angle between \rho and the x direction and can be defined as:
There are two ways to do the range rate \dot{\rho(t)} derivation:
Generally we can explicitly describe the range, \rho , as a function of time:
The range rate, \dot{\rho(t)} , is defined as time rate of change of the range, \rho , and it can be described as the time derivative of \rho :
\dot{\rho} = \frac{\partial \rho(t)}{\partial t} = \frac{ \partial}{\partial t}\sqrt{p_x(t)^2 + p_y(t)^2} = \frac{1}{2 \sqrt{p_x(t)^2 + p_y(t)^2}} (\frac{ \partial}{\partial t}p_x(t)^2 + \frac{ \partial}{\partial t}p_y(t)^2)
=\frac{1}{2 \sqrt{p_x(t)^2 + p_y(t)^2}} (2 p_x(t) \frac{ \partial}{\partial t} p_x(t) + 2 p_y(t) \frac{ \partial}{\partial t} p_y(t))
\frac{ \partial}{\partial t} p_x(t) is nothing else than v_x(t) , similarly \frac{ \partial}{\partial t} p_y(t) is v_y(t) . So we have:
\dot{\rho} = \frac{\partial \rho(t)}{\partial t} = \frac{1}{2 \sqrt{p_x(t)^2 + p_y(t)^2}} (2 p_x(t) v_x(t) + 2 p_y(t) v_y(t)) = \frac{2( p_x(t) v_x(t) + p_y(t) v_y(t))}{2 \sqrt{p_x(t)^2 + p_y(t)^2}}
=\frac{p_x(t) v_x(t) + p_y(t) v_y(t)}{ \sqrt{p_x(t)^2 + p_y(t)^2}}
For simplicity we just use the following notation:
The range rate, \dot{\rho} , can be seen as a scalar projection of the velocity vector, \vec{v} , onto \vec{\rho} . Both \vec{\rho} and \vec{v} are 2D vectors defined as:
The scalar projection of the velocity vector \vec{v} onto \vec{\rho} is defined as:
where \lvert \vec{\rho} \rvert is the length of \vec{\rho} . In our case it is actually the range, so \rho = \lvert \vec{\rho} \rvert .
The Next Quiz
h is a nonlinear function. In the next quiz I would like to check your intuition about what that means.